/*
 *  TestDriver.cpp
 *  powaah
 *
 *  Created by Tim Uusitalo on 3/30/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "TestDriver.h"

#include <ctime>

#include "Timer.h"

#include "TrackInfo.h"

using namespace TrackInfo;
using namespace MathUtility;

const float TestDriver::steerSensitivityOffset = 80.0f;
const float TestDriver::steerLock = 0.785398f;

/* Gear Changing Constants*/
const int TestDriver::gearUp[6]=
{
	9500,8500,8500,8500,86000,0
};
const int TestDriver::gearDown[6]=
{
	0,3000,5300,6000,6300,6300
};

/* Stuck constants*/
const int TestDriver::stuckTime = 25;
const float TestDriver::stuckAngle = 0.5235987; //PI/6

/* Accel and Brake Constants*/
const float TestDriver::maxSpeedDist=200.0f;
const float TestDriver::maxSpeed=360;
const float TestDriver::sin5 = 0.08716;
const float TestDriver::cos5 = 0.99619;

/**
 Steering Constants
 **/
const float TestDriver::wheelSensitivityCoeff=1;

/* ABS Filter Constants */
const float TestDriver::wheelRadius[4]={0.3179,0.3179,0.3276,0.3276};
const float TestDriver::absSlip=2.0;
const float TestDriver::absRange=3.0;
const float TestDriver::absMinSpeed=3.0;

/* Clutch constants */
const float TestDriver::clutchMax=0.5;
const float TestDriver::clutchDelta=0.05;
const float TestDriver::clutchRange=0.82;
const float TestDriver::clutchDeltaTime=0.02;
const float TestDriver::clutchDeltaRaced=10;
const float TestDriver::clutchDec=0.01;
const float TestDriver::clutchMaxModifier=1.3;
const float TestDriver::clutchMaxTime=1.5;



TestDriver::TestDriver()
: offset(0.0f), dt(0.0f)
{
	timer.startTimer();
	ds = 0.0f;
	
	lap = 0;
	if (track.load())
	{
		lap = 2;
		std::cout << "aoe" << std::endl;
	}
	
	tick = 0;

	time = 0;
	
	start_velocity = 0;
	
}

TestDriver::~TestDriver()
{
	for (unsigned int i = 0; i < accelerations.size(); i++)
	{
		std::cout << "acc: " << accelerations[i] << std::endl;
	}
}

// the drive function wiht string input and output
string TestDriver::drive(string sensors)
{
	return WrapperBaseDriver::drive(sensors);
}

// drive function that exploits the CarState and CarControl wrappers as input and output.
CarControl TestDriver::wDrive(CarState cs)
{	
	dt = 0.02f;//timer.elapsedTime();
	timer.startTimer();
	time += dt;
	tick++;

	
	//std::cout << dt << std::endl;

	//std::cout << cs.getFocus(1) << std::endl;
	



	if( ds - cs.getDistFromStart() > 25.0f && lap < 2 && this->stage == BaseDriver::WARMUP)
	{
		
		
		ds = cs.getDistFromStart();
		
		if (cs.getTrackPos() < 0.4f && cs.getTrackPos() > -0.4f)
		{
			track.record(cs, da.getWidth(), &angles[0], 0, old_cc);
		}
		
		lap++;
		
		if (lap == 2)
		{
			track.save();
			std::cout << "saving trackinfo" << std::endl;
		}
	}
	
	else if (cs.getDistFromStart() - ds > 25.0f && lap < 2 && this->stage == BaseDriver::WARMUP)
	{
		ds = cs.getDistFromStart();
		if (cs.getTrackPos() < 0.3f && cs.getTrackPos() > -0.3f)
			track.record(cs, da.getWidth(), &angles[0], 0, old_cc);
	}
	
	else if (lap >= 2)
	{
		track.update(cs);

		da.update(cs, dt, track);
	}
	
	// check if car is currently stuck
	if ( fabs(cs.getAngle()) > stuckAngle )
    {
		// update stuck counter
        stuck++;
    }
    else
    {
    	// if not stuck reset stuck counter
        stuck = 0;
    }
	
	// after car is stuck for a while apply recovering policy
    if (stuck > stuckTime)
    {
    	/* set gear and sterring command assuming car is 
    	 * pointing in a direction out of track */
    	
    	// to bring car parallel to track axis
        float steer = - cs.getAngle() / steerLock; 
        int gear=-1; // gear R
        
        // if car is pointing in the correct direction revert gear and steer  
        if (cs.getAngle()*cs.getTrackPos()>0)
        {
            gear = 1;
            steer = -steer;
        }
		
        // Calculate clutching
        clutching(cs,clutch);
		
        // build a CarControl variable and return it
        CarControl cc (0.5f,0.0,gear,steer,clutch);
        return cc;
    }
	
    else // car is not stuck
    {
    	// compute accel/brake command
        float accel = filterTLS(cs, getAccelPad(cs, old_state));
		float brake = 0.0f;
		if (cs.getTrackPos() < 1.0f && cs.getTrackPos() > -1.0f)
		{
			
			brake = filterABS(cs,da.getResultingOption().getBrake());
		}
		
//		if (accel >= 0)
//			brake = 0;

        // compute gear 
        int gear = getGear(cs);
        // compute steering
        float steer = getSteer(cs);
        
		
        // normalize steering
        if (steer < -1)
            steer = -1;
        if (steer > 1)
            steer = 1;

        // Calculate clutching
        clutching(cs,clutch);
		
		//std::cout << time << std::endl;
		/*
		if (time >= 1.0f)
		{
			
			float vel = (cs.getSpeedX()/3.6f) - start_velocity;
			
			vel = vel / time;
			
			//std::cout << time << std::endl;
			std::cout << "acceleration: " << vel <<  std::endl;
			time = 0;
			start_velocity = cs.getSpeedX()/3.6f;
		}
		 */
		
		// build a CarControl variable and return it
        CarControl cc(accel,brake,gear,steer,clutch);
		old_state = cs;
		
		old_cc = cc;


		cc.setFocus(0);

        return cc;
    }
	
}

void TestDriver::onShutDown()
{
	std::cout << "Bye bye" << std::endl;
}

void TestDriver::onRestart()
{
	std::cout << "Restarting race" << std::endl;
}

void TestDriver::init (float* const angles)
{
	
	// set angles as {-90,-75,-60,-45,-30,20,15,10,5,0,5,10,15,20,30,45,60,75,90}

	for (int i = 0; i < 5; i++)
	{
		angles[i] = -90 + i*15;
		angles[18-i] = 90 - i*15;

		this->angles[i] = -90 + i*15;
		this->angles[18-i] = 90 - i*15;

	}
	
	for (int i = 5; i < 9; i++)
	{
		angles[i] = -20+(i-5)*5;
		angles[18-i] = 20-(i-5)*5;
		
		this->angles[i] = -20+(i-5)*5;
		this->angles[18-i] = 20-(i-5)*5;
	}
		//angles[1] = -0.01f;
		
		//angles[17] = 0.01f;
	angles[9] = 0;
	this->angles[9] = 0;
}


// private

const float TestDriver::getSteer(CarState& cs)
{

	float devolver = 0.0f;

	//else
	{
		if (cs.getTrackPos() < 1.0f && cs.getTrackPos() > -1.0f)
		{
			if (lap <= 1 && this->stage == BaseDriver::WARMUP)
			{
				devolver = (2 * cs.getAngle() - (cs.getTrackPos()*0.5f));

			
			
				devolver = devolver / steerLock;
			}
			else 
			{
				
				float targetAngle = 0.0f;
			
				targetAngle = da.getResultingOption().getAngle() * -1;
			
			
				devolver = targetAngle / steerLock;
			}
		}
		 
		else 
		{
			devolver = (2 * cs.getAngle() - (cs.getTrackPos()*0.5f));
			
			devolver = devolver / steerLock;
		}

	}

	if (devolver < -1)
		devolver = -1.0f;
	if (devolver > 1)
	{
		devolver = 1;
	}

	return devolver;

}

const float TestDriver::getAccelPad(CarState& cs, CarState& old)
{


	// checks if car is out of track
    if (cs.getTrackPos() < 1.0f && cs.getTrackPos() > -1.0f)
    {
		float target_speed = 0.0f;
		{
			//target_speed = da.getResultingOption().getPosition().x * 3.6f;//da.getSpeed(cs);
			if (lap <= 1 && this->stage == BaseDriver::WARMUP)
			{
				target_speed = 35;
				target_speed = 2/(1+exp(cs.getSpeedX() - target_speed)) - 1;
				if (target_speed > 0.3f)
				{
					return 0.3f;
				}
				else
					return da.getTargetSpeed();
			}
			else 
			{
				return da.getResultingOption().getAccelerator();
			}
			
		}
		
		return 2/(1+exp(cs.getSpeedX() - target_speed)) - 1;
	}
    else
        return 0.3; // when out of track returns a moderate acceleration command
	
}

const float TestDriver::getBrake(CarState& cs, CarState& old)
{
	if (old.getTrack(9) > cs.getTrack(9))
	{
		float brake = old.getTrack(9) - cs.getTrack(9);
		
		if (brake > cs.getSpeedX())
		{
			return 1.0f;
		}
	}
	
	return 0.0f;
}

const int TestDriver::getGear(CarState& cs)
{
	int gear = cs.getGear();
    int rpm  = cs.getRpm();
	
    // if gear is 0 (N) or -1 (R) just return 1 
    if (gear<1)
        return 1;
    // check if the RPM value of car is greater than the one suggested 
    // to shift up the gear from the current one     
    if (gear <6 && rpm >= gearUp[gear-1])
        return gear + 1;
    else
    	// check if the RPM value of car is lower than the one suggested 
    	// to shift down the gear from the current one
        if (gear > 1 && rpm <= gearDown[gear-1])
            return gear - 1;
        else // otherwhise keep current gear
            return gear;
}

void TestDriver::clutching(CarState&cs, float& clutching)
{
	double maxClutch = clutchMax;
	
	// Check if the current situation is the race start
	if (cs.getCurLapTime()<clutchDeltaTime  && stage==RACE && cs.getDistRaced()<clutchDeltaRaced)
		clutch = maxClutch;
	
	// Adjust the current value of the clutch
	if(clutch > 0)
	{
		double delta = clutchDelta;
		if (cs.getGear() < 2)
		{
			// Apply a stronger clutch output when the gear is one and the race is just started
			delta /= 2;
			maxClutch *= clutchMaxModifier;
			if (cs.getCurLapTime() < clutchMaxTime)
				clutch = maxClutch;
		}
		
		// check clutch is not bigger than maximum values
		clutch = min(maxClutch,double(clutch));
		
		// if clutch is not at max value decrease it quite quickly
		if (clutch!=maxClutch)
		{
			clutch -= delta;
			clutch = max(0.0,double(clutch));
		}
		// if clutch is at max value decrease it very slowly
		else
			clutch -= clutchDec;
	}

}

const float TestDriver::getAllowedSpeed(CarState& cs)
{
	// checks if car is out of track
    if (cs.getTrackPos() < 1 && cs.getTrackPos() > -1)
    {
        // reading of sensor at +5 degree w.r.t. car axis
        float rxSensor=cs.getTrack(10);
        // reading of sensor parallel to car axis
        float cSensor=cs.getTrack(9);
        // reading of sensor at -5 degree w.r.t. car axis
        float sxSensor=cs.getTrack(8);
		
        float targetSpeed;
		
        // track is straight and enough far from a turn so goes to max speed
        if (cSensor>maxSpeedDist || (cSensor>=rxSensor && cSensor >= sxSensor))
            targetSpeed = maxSpeed;
        else
        {
			float arc = cs.getAngle();
			
			arc /= pi * 0.5f;
			
			//float r = 0.0f;
        }
		
        // accel/brake command is expontially scaled w.r.t. the difference between target speed and current one
		
        return 2/(1+exp(cs.getSpeedX() - targetSpeed)) - 1;
    }
    else
        return 0.3; // when out of track returns a moderate acce
}

const float TestDriver::filterABS(CarState& cs, float brake)
{
	// convert speed to m/s
	float speed = cs.getSpeedX() / 3.6;
	// when spedd lower than min speed for abs do nothing
    if (speed < absMinSpeed)
        return brake;
    
    // compute the speed of wheels in m/s
    float slip = 0.0f;
    for (int i = 0; i < 4; i++)
    {
        slip += cs.getWheelSpinVel(i) * wheelRadius[i];
    }
    // slip is the difference between actual speed of car and average speed of wheels
    slip = speed - slip/4.0f;
    // when slip too high applu ABS
    if (slip > absSlip)
    {
        brake = brake - (slip - absSlip)/absRange;
    }
    
    // check brake is not negative, otherwise set it to zero
    if (brake<0)
    	return 0;
    else
    	return brake;
	
}

const float TestDriver::filterTLS(CarState& cs, const float accel)
{
	if (cs.getSpeedX() < 3.0f)
		return accel;
	
	float finalAccelPad = accel;
	
	float slip = cs.getSpeedX() / ((cs.getWheelSpinVel(2) + cs.getWheelSpinVel(3)) * wheelRadius[2] * 0.5f);
	
	if (slip < 0.9f)
	{
		finalAccelPad = 0.0f;
	}
	
	return finalAccelPad;
}
